﻿
#include "vehicle-func.h"

using namespace vehicle_func;

OptionalDouble vehicle_func::calcCurvature(Double speed, OptionalDouble yawRate, OptionalDouble steer, OptionalDouble steerAngleRatio, OptionalDouble wheelBase)
{
	const Double YAWRATE_MIN = 0.1;
	const Double SEGMENT_LOW = 10;
	const Double SEGMENT_HIGH = 20;

	OptionalDouble curvByYawRate;
	if (yawRate.valid)
	{
		if (math::abs(speed) > YAWRATE_MIN) curvByYawRate = yawRate.value * math::DEG2RAD / (speed / 3.6);
	}

	OptionalDouble curvBySteerAngle;
	if (steer.valid && steerAngleRatio.valid && wheelBase.valid)
	{
		curvBySteerAngle = math::sin(steer.value / steerAngleRatio.value) / wheelBase.value;
	}
	
	if (curvByYawRate.valid)
	{
		if (curvBySteerAngle.valid)
		{
			if (speed < SEGMENT_LOW) return curvBySteerAngle;
			else if (speed > SEGMENT_HIGH) return curvByYawRate;
			else
			{
				Double wSteer = (SEGMENT_HIGH - speed) / (SEGMENT_HIGH - SEGMENT_LOW);
				Double wYawRate = 1.0 - wSteer;
				return curvBySteerAngle.value * wSteer + curvByYawRate.value * wYawRate;
			}
		}
		else return curvByYawRate;
	}
	else
	{
		if (curvBySteerAngle.valid) return curvBySteerAngle;
		else return OptionalDouble();
	}
}

Array<Point2D> vehicle_func::calcTrajectory(Optional<VehicleSample> previousVS, Double currentTimestamp)
{
	if (!previousVS.isValid()) return Array<Point2D>::scalar(Point2D(0, 0));

	VehicleSample& previous = previousVS.refValue();

	Double yawRate = 0;
	if (previous->yawRate.valid) yawRate = previous->yawRate.value;
	else if (previous->curvature.valid && math::abs(previous->speed) > 1)
	{
		yawRate = previous->curvature.value * (previous->speed / 3.6) * math::RAD2DEG;
	}

	Double deltaT = currentTimestamp - previous.timestamp().offset;
	Double mx = previous->speed / 3.6 * deltaT;
	Double theta = yawRate * deltaT;
	Double my = previous->speed / 3.6 * math::tan(theta / 2.0) * deltaT;
	Double cosTheta = math::cos(theta);
	Double sinTheta = math::sin(theta);

	Array<Point2D> output(math::min(500u, previous->trajectory.size() + 1));

	output[0].x = 0.0;
	output[0].y = 0.0;

	for (UInt l = 0; l < (output.size() - 1); l++)
	{
		Double preTx = previous->trajectory[l].x - mx;
		Double preTy = previous->trajectory[l].y - my;

		output[l + 1].x = (Float)(preTx * cosTheta + preTy * sinTheta);
		output[l + 1].y = (Float)(-preTx * sinTheta + preTy * cosTheta);
	}

	return output;
}